/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_turn_around_once.cpp
 * @Description  : 实现外设旋转操作的类 OperationTurnAroundOnce。
 *                 该类继承自Operation类，主要功能包括初始化、启动、执行、结束旋转操作，并处理相关的反馈和错误。
 *                 代码中包含对相机数据和里程计数据的处理，以实现外设精准的旋转和位置调整。
 *                 通过多个阶段的状态机控制旋转操作的流程，确保外设能够准确地完成一次旋转任务。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:42:40
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "operation_turn_around_once.h" 
#include "tools.h"
#include "public_helper.h"
#include <eigen3/Eigen/Eigen>
#include "public_kernel.h"
#include "tf2/utils.h"
// #include <math.h>
using namespace Eigen;
constexpr int ODOM_DATA_TIME_OUT = 100;	   // 100MS
constexpr int CAMERA_DATA_TIME_OUT = 300;  // 100MS
constexpr int PERIPH_DATA_TIME_OUT = 500;  // 500MS
constexpr int CAMERA_LOST_LIMIT = 50;      // 50
///////////////////////////////class OperationTurnAroundOnce implement///////////////////////////////////////////////
/**
 * @brief 构造函数
 *
 * OperationTurnAroundOnce 类是继承自 Operation 类的子类，用于实现全方位旋转操作。
 *
 * @param action 操作指令，类型为 agv_msgs::msg::Action
 */
OperationTurnAroundOnce::OperationTurnAroundOnce(const agv_msgs::msg::Action& action)
	: Operation(action)
{
	phase_ = PHASE_READY_START;
}

/**
 * @brief 析构函数，用于销毁 OperationTurnAroundOnce 对象
 *
 * 在对象销毁时调用，执行一些清理工作，例如调用 LoadEnd 函数。
 */
OperationTurnAroundOnce::~OperationTurnAroundOnce()
{
	LoadEnd();
}


/**
 * @brief 初始化函数
 *
 * 初始化ROS相关值，并返回初始化结果。
 *
 * @return 如果初始化成功，返回true；否则返回false。
 */
bool OperationTurnAroundOnce::Init()
{
	bool ret = InitRosValue();
	return ret;
}

/**
 * @brief 准备开始
 *
 * @return 如果准备成功，返回true；否则返回false。
 */
bool OperationTurnAroundOnce::ReadyStart()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	if (!TryCycleMarkCheck()) {
		DEBUG_ERROR_OUT("OperationTurnAroundOnce: qrId is not Valid");
		return false;
	}

	// 做当前动作时，设置节点bit位
	FirstSetPathBitsData();
	SetPhase(PHASE_READY_START);
	return true;
}

/**
 * @brief 开始执行动作
 *
 * @return 返回操作是否成功启动。如果成功启动，返回true；否则返回false。
 */
bool OperationTurnAroundOnce::StartAction()
{
	std::lock_guard<std::mutex> locker(mutex_);

	finished_ = false;
#ifdef AGV_DEBUG_NAV
	return true;
#endif

	bool ret = true;
	DEBUG_OUT("OperationTurnAroundOnce start :ret:" << ret);
	return ret;
}

/**
 * @brief 执行函数
 *
 * @return 执行成功返回true，否则返回false。
 */
bool OperationTurnAroundOnce::Work()
{
	std::lock_guard<std::mutex> locker(mutex_);

#ifdef AGV_DEBUG_NAV
	return m_uTotalCount++ > 100; // 5s-500
#endif
	return CycleProc();
}

/**
 * @brief 结束动作函数
 *
 * 停止当前动作并返回 true。
 *
 * @return 如果成功停止当前动作，则返回 true；否则返回 false。
 */
bool OperationTurnAroundOnce::EndAction()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	Stop();
	return true;
}

/////////////////////////////////////action消息回调函数//////////////////////////////////////////

/**
 * @brief 外设反馈回调函数
 *
 * 该函数用于处理外设反馈数据。
 *
 * @param feedback 外设反馈数据指针
 */
void OperationTurnAroundOnce::FeedbackCb(GoalHandlePeripheral::SharedPtr,
      const std::shared_ptr<const Peripheral::Feedback> feedback)
{
	int cur_state = feedback->state;
	float cur_percent = feedback->percent_complete;
	(void)(cur_percent);
	// if (cur_percent >= 0.95f || cur_percent < 0.1f)
	{
		DEBUG_OUT("OperationTurnAroundOnce up; cur_state:" << cur_state << ",cur_percent:" << cur_percent);
	}
}

/**
 * @brief 当操作完成时，此回调函数被调用。
 *
 * 当操作完成时，此函数会根据结果更新内部状态，并输出相应的日志信息。
 *
 * @param state 目标状态，表示操作的目标状态。
 * @param result 操作结果，包含操作是否成功以及相关的消息。
 */
void OperationTurnAroundOnce::DoneCb(const GoalHandlePeripheral::WrappedResult & result)
{
	std::lock_guard<std::mutex> locker(mutex_);
    auto& result_msg = result.result;
	DEBUG_WARN_OUT("OperationTurnAroundOnce up ;Peripheral Success :" <<
		 result_msg->success << ", message: " << result_msg->message);
	bool peripheral_result = result_msg->success;

	if (!peripheral_result) {
		finished_ = true;
		DEBUG_WARN_OUT("OperationTurnAroundOnce up;action finsh ok");
	} else {
		load_phase_ = LOAD_INIT;
		DEBUG_ERROR_OUT(" OperationTurnAroundOnce up; action do failed:ready restart");
	}
}

/**
 * @brief ActiveCb
 *
 * 当目标刚变为激活状态时调用的回调函数。
 */
void OperationTurnAroundOnce::ActiveCb(GoalHandlePeripheral::SharedPtr goal_handle)
{
	DEBUG_WARN_OUT("OperationTurnAroundOnce Goal just went active");
}

/**
 * @brief 处理加载动作周期
 *
 * 根据当前加载阶段执行相应的加载动作，并在加载成功后更新加载阶段。
 *
 * @return 如果加载成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::HandleLoadAcitonCycle()
{
	bool ret = false;
	switch (load_phase_) {
		case LOAD_INIT:
			if (LoadInit()) {
				load_phase_ = LOAD_READY_START;
				DEBUG_OUT("load run stage: enter LOAD_READY_START");
			}
			break;
		case LOAD_READY_START:
			if (LoadReadyStart()) {
				load_phase_ = LOAD_START;
				DEBUG_OUT("load run stage: enter LOAD_START");
			}
			break;
		case LOAD_START:
			if (LoadStart()) {
				load_phase_ = LOAD_WORK;
				DEBUG_OUT("load run stage: enter LOAD_WORK");
			}
			break;
		case LOAD_WORK:
			if (LoadWork()) {
				load_phase_ = LOAD_READY_END;
				DEBUG_OUT("load run stage: enter LOAD_READY_END");
			}
			break;
		case LOAD_READY_END:
			if (LoadEnd()) {
				load_phase_ = LOAD_END;
				DEBUG_OUT("load run stage: enter LOAD_END");
			}
			break;
		case LOAD_END:
			DEBUG_OUT("load  run ok: LOAD_END");
			ret = true;
			break;
		case LOAD_ERROR:
			DEBUG_OUT("load action do error! load run stage: LOAD_ERROR");
			break;
		default:
			break;
	}

	static int times = 0;
	if (times++ > 20) {
		times = 0;
		DEBUG_OUT("cur load phase:" << load_phase_);
	}

	if (ret) {
		// SetPhase(PHASE_FINISH);
		DEBUG_OUT("cur load ok ready to exit cycle:");
	}

	return ret;
}

/**
 * @brief 初始化加载方法
 *
 * 该方法用于初始化加载操作，并准备执行旋转全方位操作。
 *
 * @return 如果初始化成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::LoadInit()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	delay_count_ = 0;
	if (!OpClient::client_) {
		DEBUG_ERROR_OUT("up:client_  is null");
		return false;
	}

	if (!OpClient::client_->action_server_is_ready()) {
		DEBUG_ERROR_OUT("up:action client isServerConnected is failed");
		return false;
	}
	DEBUG_OUT("up:Init is ok; ready to clear(end) last action");
	LoadEnd(); // add at 2023-2-13

	return true;
}

/**
 * @brief 加载并准备启动AMR转盘控制
 *
 * @return 准备完成返回true，否则返回false
 */
bool OperationTurnAroundOnce::LoadReadyStart()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	const int delay_ms = 800, cycle = 50;
	const int max_delay_count = delay_ms / cycle; // 800ms /50ms = 16;
	if (delay_count_++ >= max_delay_count) {
		DEBUG_OUT("up:before start delay ms:" << delay_ms << ",delay_count_:" << delay_count_);
		return true;
	}
	return false;
}

/**
 * @brief 启动负载操作
 *
 * 该函数用于启动负载操作。
 *
 * @return 如果操作成功启动，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::LoadStart()
{
	finished_ = false;
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	if (!OpClient::client_ || !OpClient::client_->action_server_is_ready()) {
		DEBUG_ERROR_OUT("up:StartAction failed");
		return false;
	}

	try {
		auto goal_msg = Peripheral::Goal();
        goal_msg.goal_action = action_;        // 直接发送调度格式的action
        auto send_goal_options = rclcpp_action::Client<Peripheral>::SendGoalOptions();
        send_goal_options.goal_response_callback =
            std::bind(&OperationTurnAroundOnce::ActiveCb, this, std::placeholders::_1);
        send_goal_options.feedback_callback =
            std::bind(&OperationTurnAroundOnce::FeedbackCb, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback =
            std::bind(&OperationTurnAroundOnce::DoneCb, this, std::placeholders::_1);
		
        auto goal_handle_future = OpClient::client_->async_send_goal(goal_msg, send_goal_options);
		DEBUG_OUT("up:Operation start ok");
	} catch (const std::exception &e) {
		DEBUG_ERROR_OUT("action:" << e.what());
	} catch (...) {
		DEBUG_ERROR_OUT("start action failed");
	}
	return true;
}

/**
 * @brief 执行负载操作
 *
 * 根据finished_标志位返回true或false。
 *
 * @return finished_为true，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::LoadWork()
{
#ifdef AGV_DEBUG_NAV
	return m_uTotalCount++ < 100; // 5s-500
#endif
	return finished_;
}

/**
 * @brief 负载操作结束
 *
 * 该函数在AGV负载操作完成后调用，用于处理一些结束时的操作。
 *
 * @return 返回true表示操作成功，否则返回false
 */
bool OperationTurnAroundOnce::LoadEnd()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif

	//.if (OpClient::client_ && OpClient::client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
	if (OpClient::client_)
	{
		auto future = OpClient::client_->async_cancel_all_goals();
		DEBUG_OUT("up:#2 stopTrackingGoal");
	}

	return true;
}

////////////////////////////////////////原地矫正逻辑/////////////////////////////////////////
//
// 订阅ros相关话题
//
bool OperationTurnAroundOnce::InitRosValue()
{
	return true;
}

//  获取载货状态
/**
 * @brief 获取载货状态
 *
 * @return 返回true表示操作成功，否则返回false
 */
bool OperationTurnAroundOnce::GetLoadStatus(bool &is_load)
{
	const auto &op_data = GetOpData();
	auto perphStatus = op_data.periph_status_msg;
	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(perphStatus.header.stamp, ROSTime(), PERIPH_DATA_TIME_OUT)) {
		DEBUG_ERROR_OUT("Turn around op: GetLoadStatus time out");
		return false;
	}
	is_load = perphStatus.loaded;
	return true;
}

/**
 * @brief 从地面相机数据中获取位置和角度信息
 *
 * 从操作数据中获取地面相机的位置和方向信息，并将其转换为相对位置和方向，
 * 然后将这些信息存储在提供的引用参数中。
 *
 * @param pos_x 地面相机在x轴上的相对位置（输出参数）
 * @param pos_y 地面相机在y轴上的相对位置（输出参数）
 * @param angle 地面相机的方向角度（输出参数）
 *
 * @return 如果成功获取数据，则返回true；否则返回false
 */
bool OperationTurnAroundOnce::GetGroundCameraData(float &pos_x, float &pos_y, float &angle)
{
	const auto &op_data = GetOpData();
	auto ground_camera_pose = op_data.ground_camera_pose_msg;

	Posture pose;
	bool ret = ToRelativePos(ground_camera_pose, relative_zero_pos_, pose);
	pos_x = pose.x_;
	pos_y = pose.y_;
	angle = pose.theta_;
	DEBUG_OUT("GetGroundCameraData:ret=" << ret << ",pos_x=" << pos_x << ",pos_y=" << pos_y << ",angle=" << angle);
	return ret;
}

/**
 * @brief 获取货架摄像头的数据
 *
 * 获取货架摄像头的位置（x, y）和角度（angle）。
 *
 * @param pos_x 货架摄像头的x坐标（输出参数）
 * @param pos_y 货架摄像头的y坐标（输出参数）
 * @param angle 货架摄像头的角度（输出参数）
 *
 * @return 如果获取数据成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::GetRackCameraData(float &pos_x, float &pos_y, float &angle)
{
	const auto &op_data = GetOpData();
	auto rack_camera_pose = op_data.rack_camera_pose_msg;
	Posture pose;
	bool ret = ToRelativePos(rack_camera_pose, relative_zero_pos_, pose);
	pos_x = pose.x_;
	pos_y = pose.y_;
	angle = pose.theta_;
	DEBUG_OUT("GetRackCameraData:ret=" << ret << ",pos_x=" << pos_x << ",pos_y=" << pos_y << ",angle=" << angle);
	return ret;
}

//
// 判断数据是否超时
//
// bool OperationTurnAroundOnce::IsTimeOut(const rclcpp::Time &data_time, const rclcpp::Time &cur_time, const double &max_time_ms)
// {
// 	double time_diff_ms = (cur_time.toSec() - data_time.toSec()) * 1000.0;
// 	bool ret = time_diff_ms > max_time_ms;
// 	if (ret)
// 	{
// 		DEBUG_ERROR_OUT("data TimeOut:time_diff_ms=" << time_diff_ms << ",max_time_ms=" << max_time_ms);
// 	}
// 	return ret;
// }

/**
 * @brief 获取里程计数据
 *
 * 从操作数据中获取里程计信息，包括线速度和角速度。
 *
 * @param vel 输出的线速度和角速度结构体
 * @param theta 输出的航向角
 *
 * @return bool 如果获取数据成功，返回true；否则返回false
 */
bool OperationTurnAroundOnce::GetOdomData(float &vx, float &vz, float &theta)
{
	const auto &op_data = GetOpData();
	auto odom_msg = op_data.odom_msg;

	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(odom_msg.header.stamp, ROSTime(), ODOM_DATA_TIME_OUT)) {
		DEBUG_ERROR_OUT("omi Turn around op: GetOdomData time out");
		return false;
	}
	geometry_msgs::msg::Pose data_pos = odom_msg.pose;
	geometry_msgs::msg::Twist data_speed = odom_msg.twist;
	vx = data_speed.linear.x;
	vz = data_speed.angular.z;
	theta = 0.0f;
	try {
		theta = tf2::getYaw(data_pos.orientation);
	} catch (...) {
		DEBUG_ERROR_OUT("tf2::getYaw(odom.orientation) failed");
		return false;
	}

	return true;
}

/**
 * @brief 参数
 *
 */
void OperationTurnAroundOnce::SetDefaultValue()
{	
	auto node = ROSNode();
	// 参数读取
	node->get_parameter_or("/agv_navigate_node/turn_around/threshold_x", limit_x_, 0.008f);
    node->get_parameter_or("/agv_navigate_node/turn_around/threshold_y", limit_y_, 0.008f);
    node->get_parameter_or("/agv_navigate_node/turn_around/threshold_theta", limit_theta_, 0.6f);
    node->get_parameter_or("/agv_navigate_node/turn_around/max_spin_vel", max_spin_vel_, 0.4f);
    node->get_parameter_or("/agv_navigate_node/turn_around/min_spin_vel", min_spin_vel_, 0.1f);
    node->get_parameter_or("/agv_navigate_node/turn_around/vel_x", velocity_, 0.03f);
    node->get_parameter_or("/agv_navigate_node/turn_around/max_adjust_times", max_adjust_times_, 6);
    node->get_parameter_or("/agv_navigate_node/turn_around/parallel_axis_k", division_k_, 50);
   
	limit_x_ = fabs(limit_x_);
	limit_y_ = fabs(limit_y_);
	limit_theta_ = (fabs(limit_theta_) / 180.0f) * PI;
	division_k_ = std::max(division_k_, 10);
	DEBUG_OUT("op:turnAround param:fLimitX=" << limit_x_
											 << ",fLimitY=" << limit_y_
											 << ",fLimitTheta=" << limit_theta_
											 << ",m_fMaxSpinVel=" << max_spin_vel_
											 << ",m_fMinSpinVel=" << min_spin_vel_
											 << ",m_fVelX=" << velocity_
											 << ",m_nMaxAdjustTimes=" << max_adjust_times_
											 << ",division_k_=" << division_k_
											 << ",NearlyXAxisValue=" << (limit_x_ / division_k_)
											 << ",NearlyYAxisValue=" << (limit_y_ / division_k_));

	ResetStatus();
}

/**
 * @brief 重置状态
 *
 * 重置操作状态，包括清除相关变量和设置初始值。
 */
void OperationTurnAroundOnce::ResetStatus()
{
	// 清除变量
	current_adjust_times_ = 0;
	last_time_ = GetTickCount();
	current_line_progress_ = 0;
	cycle_ = 0.05f;
	current_camera_type_ = CAMERA_CHECK_RACK;
	// 取货变量
	load_phase_ = LOAD_INIT;
}

//
//   StartPreTune
//
// void OperationTurnAroundOnce::StartPreTune()
// {
// 	SetDefaultValue();
// 	SetPhase(PHASE_START);
// }

/**
 * @brief 设置当前阶段
 *
 * 设置操作的当前阶段，并清除数据丢失标志和旋转标志。
 *
 * @param phase 要设置的阶段编号
 */
void OperationTurnAroundOnce::SetPhase(int phase)
{
	DEBUG_OUT("SetPhase: lastPhase=" << phase_ << ",newPhase=" << phase);
	phase_ = phase;
	ClearCameraLostFlag();
	ClearSpinFlag();
}

/**
 * @brief 清除自旋标志
 *
 * 该函数用于清除自旋标志，使对象重新进入准备旋转的初始状态。
 */
void OperationTurnAroundOnce::ClearSpinFlag()
{
	spin_phase_ = SPIN_READY_START;
	last_real_angle_ = 0.0f;
	turned_angle_ = 0.0f;
	micro_angle_diff_ = 0.0f;
	current_line_progress_ = 0; // 必须直线完成后清除此变量
}

//
//  预先检测
//  return RET_GET_LOAD_DATA_FAILED;
//  return RET_LOAD_STATUS_ERROR;
//
// 	return RET_GET_DATA_FAILED_ERROR;
//  return RET_NEXT_STAGE;
//
int OperationTurnAroundOnce::ReadyStartTune()
{
	SetDefaultValue();
	bool is_load = false;
	if (!GetLoadStatus(is_load)) { // 	if (!GetLoadStatus(bLoad)){
		DEBUG_ERROR_OUT("errro:ReadyStartTune tune around action:GetLoadStatus time out");
		return RET_GET_LOAD_DATA_FAILED;
	}

	if (is_load) {
		DEBUG_ERROR_OUT("error: ReadyStartTune tune around curLoad is already load=" << is_load);
		return RET_LOAD_STATUS_ERROR;
	}

	int ret = SetRelativePos();

	if (ret != RET_NEXT_STAGE) {
		DEBUG_ERROR_OUT("error: ReadyStartTune tune SetRelativePos;ret=" << ret);
		return ret;
	}
	// ret = CalcTurnData();
	DEBUG_OUT("ReadyStartTune once");
	return ret;
}

//
//  开启
// return RET_X_Y_ANGLE_TARGET;
// return RET_X_Y_TARGET;
// return RET_NEXT_STAGE;
// return RET_GET_DATA_FAILED;
//
int OperationTurnAroundOnce::StartTune()
{
	int ret = CalcTurnData();
	DEBUG_OUT("Start tune around action:CalcTurnData once");
	return ret;
}

/////////////////////////新改逻辑/////////////////////////////
/**
 * @brief 计算转向数据
 *
 * 此函数负责从摄像机读取货架的姿态误差，并根据误差值决定下一步操作。
 *
 * @return 返回值表示当前处理状态：
 *         - RET_X_Y_ANGLE_TARGET: 误差已小于门限，位置核查通过
 *         - RET_X_Y_TARGET: 仅X和Y方向误差已小于门限，需要旋转调整角度
 *         - RET_NEXT_STAGE: 需要继续调整位置
 *         - RET_GET_DATA_FAILED: 无法从摄像机获取数据
 */
int OperationTurnAroundOnce::CalcTurnData()
{
	// 复位上一个阶段的状态，为再次调整做准备
	// ResetStatus();
	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	bool ret = ReadCameraPos(current_camera_type_, x, y, angle);
	if (ret) {
		ClearCameraLostFlag();
		// 如误差已小于门限，则位置核查通过
		if (IsTurnTarget(x, y, angle)) {
			Stop();
			return RET_X_Y_ANGLE_TARGET;
		} else if (IsXYTurnTarget(x, y)) {
			move_data_.start_spin_back_angle = -angle;
			return RET_X_Y_TARGET;
		} else {
			camera_pos_.x = x;
			camera_pos_.y = y;
			camera_pos_.theta = angle;
			DEBUG_OUT("set cameraPos:x=" << x << ",y=" << y << ",angle=" << angle);
			SetMoveValue(camera_pos_);
			return RET_NEXT_STAGE;
		}
	} else { // 如未能从摄像机得到货架位置信息
		Stop();
		CheckCameraLostData();
	}

	return RET_GET_DATA_FAILED;
}

/**
 * @brief 判断给定坐标是否在XY轴旋转的目标范围内
 *
 * 根据给定的X和Y坐标值，判断这些坐标是否满足在XY轴旋转的目标范围内。
 *
 * @param x X坐标值
 * @param y Y坐标值
 *
 * @return 如果给定的坐标在目标范围内，则返回true；否则返回false
 */
bool OperationTurnAroundOnce::IsXYTurnTarget(float x, float y)
{
	bool ret = false;

	ret = fabs(x) < limit_x_ && fabs(y) < limit_y_;
	DEBUG_OUT("IsXYTurnTarget;ret:" << ret << ",x:" << x << ",limit_x_=" << limit_x_ << ",y=" << y << ",limit_y_=" << limit_y_);

	return ret;
}


/**
 * @brief 判断是否达到旋转目标,如误差已小于门限，则位置核查通过
 *
 * 根据给定的x、y坐标和旋转角度，判断是否达到了旋转目标。
 *
 * @param x x坐标
 * @param y y坐标
 * @param angle 旋转角度
 *
 * @return 如果达到了旋转目标，则返回true；否则返回false
 */
bool OperationTurnAroundOnce::IsTurnTarget(float x, float y, float angle)
{
	bool ret = false;

	ret = fabs(x) < limit_x_ && fabs(y) < limit_y_ && fabs(angle) < limit_theta_;
	DEBUG_OUT("IsTurnTarget;ret:" << ret << ",x:" << x << ",limit_x_=" << limit_x_ << ",y=" << y << ",limit_y_=" << limit_y_ << ",angle=" << angle
								  << ",limit_theta_=" << limit_theta_);

	return ret;
}

/**
 * @brief 设置移动参数
 *
 * 根据给定的摄像机位置（CAMERA_POS），设置相应的移动数据。
 *
 * @param pos 摄像机位置结构体
 */
void OperationTurnAroundOnce::SetMoveValue(CAMERA_POS pos)
{
	pos.theta = Angle::NormAngle2(pos.theta); //[-pi,pi]
	bool x_sign = pos.x >= 0.0f;
	bool y_sign = pos.y >= 0.0f;
	// bool bAngSign = pos.m_fTheta >= 0.0f;
	division_k_ = std::max(division_k_, 1); // 防止0数值崩溃
	bool x_zero = fabs(pos.x) < (limit_x_ / division_k_);
	bool y_zero = fabs(pos.y) < (limit_y_ / division_k_);
	// bool bAngZero = fabs(pos.m_fTheta) < 0.00001f;
	DEBUG_OUT("SetMoveValue:CAMERA_POS:X=" << pos.x << ",Y=" << pos.y << ",Theta=" << pos.theta);
	// #1 如果X偏差忽略不计
	if (x_zero) {
		if (y_sign) {
			float target_angle = -PI_2;
			move_data_.start_spin_angle = Angle::NormAngle2(target_angle - pos.theta); // 顺时针+前进
			move_data_.frist_line = pos.y;
			move_data_.start_spin_back_angle = -target_angle;
		} else {
			float target_angle = PI_2;
			move_data_.start_spin_angle = Angle::NormAngle2(target_angle - pos.theta); // 逆时针+前进
			move_data_.frist_line = -pos.y;
			move_data_.start_spin_back_angle = -target_angle;
		}

		DEBUG_OUT("bXZero=TRUE;"
				  << ",y_sign=" << y_sign << ",start_spin_angle=" << move_data_.start_spin_angle
				  << ",frist_line=" << move_data_.frist_line << ",start_spin_back_angle=" << move_data_.start_spin_back_angle);
		return;
	}

	// #2 如果Y偏差忽略不计
	if (y_zero) {
		move_data_.start_spin_angle = Angle::NormAngle2(-pos.theta); // 在x轴正轴，旋转+倒退,// 在x轴负轴，旋转+前进
		move_data_.frist_line = -pos.x;
		move_data_.start_spin_back_angle = 0.0f;

		DEBUG_OUT("bYZero=TRUE;"
				  << ",x_sign=" << x_sign << ",start_spin_angle=" << move_data_.start_spin_angle
				  << ",frist_line=" << move_data_.frist_line << ",start_spin_back_angle=" << move_data_.start_spin_back_angle);
		return;
	}

	// 如果X和Y都不为0

	// #1 第一象限
	float rad = atan2(fabs(pos.y), fabs(pos.x));
	rad = fabs(Angle::NormAngle2(rad)); //[-pi,pi]
	float lenght = sqrt(Square(pos.x) + Square(pos.y));

	if (x_sign && y_sign)
	{

		move_data_.start_spin_angle = Angle::NormAngle2(rad - pos.theta); // 第一象限 逆时针+倒退
		move_data_.frist_line = -lenght;
		move_data_.start_spin_back_angle = -rad;
		DEBUG_OUT("first quadrant:"
				  << ",rad=" << rad
				  << ",start_spin_angle=" << move_data_.start_spin_angle
				  << ",frist_line=" << move_data_.frist_line << ",start_spin_back_angle="
				  << move_data_.start_spin_back_angle);

		return;
	}
	// #2 第二象限
	if (!x_sign && y_sign) {
		move_data_.start_spin_angle = Angle::NormAngle2(-(rad + pos.theta)); // 第二象限 顺时针+前进
		move_data_.frist_line = lenght;
		move_data_.start_spin_back_angle = rad;

		DEBUG_OUT("second quadrant:"
				  << ",rad=" << rad
				  << ",start_spin_angle=" << move_data_.start_spin_angle
				  << ",frist_line=" << move_data_.frist_line << ",start_spin_back_angle="
				  << move_data_.start_spin_back_angle);

		return;
	}

	// #3 第三象限
	if (!x_sign && !y_sign) {

		move_data_.start_spin_angle = Angle::NormAngle2(rad - pos.theta); // 第三象限 逆时针+前进
		move_data_.frist_line = lenght;
		move_data_.start_spin_back_angle = -rad;

		DEBUG_OUT("third quadrant:"
				  << ",rad=" << rad
				  << ",start_spin_angle=" << move_data_.start_spin_angle
				  << ",frist_line=" << move_data_.frist_line << ",start_spin_back_angle="
				  << move_data_.start_spin_back_angle);

		return;
	}

	// #4 第四象限
	if (x_sign && !y_sign) {

		move_data_.start_spin_angle = Angle::NormAngle2(-(rad + pos.theta)); // 第四象限 顺时针+倒退
		move_data_.frist_line = -lenght;
		move_data_.start_spin_back_angle = rad;

		DEBUG_OUT("fourth quadrant:"
				  << ",rad=" << rad
				  << ",start_spin_angle=" << move_data_.start_spin_angle
				  << ",frist_line=" << move_data_.frist_line << ",start_spin_back_angle="
				  << move_data_.start_spin_back_angle);

		return;
	}
}

/**
 * @brief 执行第一次旋转
 *
 * 执行从初始角度开始的第一次旋转操作。
 *
 * @return bool 如果旋转成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::FirstSpin()
{
	bool ret = SpinMove(move_data_.start_spin_angle);
	return ret;
}

/**
 * @brief 直线移动
 *
 * 直线移动到指定位置。
 *
 * @return 如果移动成功返回 true，否则返回 false。
 */
bool OperationTurnAroundOnce::MoveLine()
{
	bool ret = StraightMove(move_data_.frist_line);
	return ret;
}

/**
 * @brief 第二次旋转操作
 *
 * 执行第二次旋转操作，并在满足限定条件时停止旋转。
 *
 * @return 如果旋转成功并且满足停止条件，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::SecondSpin()
{
	bool ret = SpinMove(move_data_.start_spin_back_angle);

	float x, y, angle;
	bool read = ReadCameraPos(current_camera_type_, x, y, angle);
	if (read) {
		if (AngleDiff(0, angle) < limit_theta_) {
			Stop();
			ret = true;
			DEBUG_OUT("SecondSpin:target ok angle=" << angle);
		}
	}
	return ret;
}

/**
 * @brief 执行第三次旋转操作
 *
 * 该函数执行第三次旋转操作，并根据相机位置和角度判断是否需要停止旋转。
 *
 * @return 若旋转完成并满足停止条件，返回 true；否则返回 false。
 */
bool OperationTurnAroundOnce::ThirdSpin()
{
	bool ret = SpinMove(move_data_.end_spin_angle);

	float x, y, angle;
	bool read = ReadCameraPos(current_camera_type_, x, y, angle);
	if (read) {
		if (AngleDiff(0.0f, angle) < limit_theta_) {
			Stop();
			ret = true;
			DEBUG_OUT("SecondSpin:target ok angle=" << angle);
		}
	}
	return ret;
}

/**
 * @brief 检查XY角度是否满足要求
 *
 * 从摄像机读取货架的姿态误差，判断误差是否在允许范围内，若不满足要求则进行相应处理
 *
 * @return 返回值表示操作结果
 *         - RET_NEXT_STAGE：位置核查通过，进入下一阶段
 *         - RET_RESTART：位置核查未通过，但未达到最大调整次数，重新调整
 *         - RET_MAX_TIMES_ERROR：位置核查未通过，且已达到最大调整次数
 *         - RET_GET_DATA_FAILED：从摄像机获取货架位置信息失败
 */
int OperationTurnAroundOnce::CheckXYAngle()
{
	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	bool read = ReadCameraPos(current_camera_type_, x, y, angle);
	if (read) {
		ClearCameraLostFlag();
		// 如误差已小于门限，则位置核查通过
		if (IsTurnTarget(x, y, angle)) {
			Stop();
			return RET_NEXT_STAGE;
		} else {
			if (current_adjust_times_ < max_adjust_times_) {
				// StartPreTune();
				current_adjust_times_++;
				DEBUG_OUT("ready adjust agin: StartPreTune"
						  << ",nCurAdjustTimes:" << current_adjust_times_ << ",m_nMaxAdjustTimes=" << max_adjust_times_);
				return RET_RESTART;
			} else {
				Stop();
				SetMaxTimesError(true);
				DEBUG_ERROR_OUT("OperationTurnAroundOnce: failed: nCurAdjustTimes:" << current_adjust_times_ << ",m_nMaxAdjustTimes=" << max_adjust_times_);
				return RET_MAX_TIMES_ERROR;
			}
		}
	} else {// 如未能从摄像机得到货架位置信息
		Stop();
		CheckCameraLostData();
		return RET_GET_DATA_FAILED;
	}

	DEBUG_OUT("OperationTurnAroundOnce"
			  << ",nCurAdjustTimes:" << current_adjust_times_);
	return RET_GET_DATA_FAILED;
}

/**
 * @brief 设置最大错误次数
 *
 * 当错误次数超过最大值时，设置任务错误信息
 *
 * @param occur 是否发生错误
 */
void OperationTurnAroundOnce::SetMaxTimesError(bool occur)
{
	std::string msg = "top reader;Exceed max corrections: failed: curTimes:" + std::to_string(current_adjust_times_) + ",maxTims=" + std::to_string(max_adjust_times_);
	msg = occur ? msg : "";
	SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), msg, agv_msgs::msg::AGVEvent::ALARM, occur);
}

/**
 * @brief 更新位置处理函数
 *
 * 该函数用于更新位置处理，并返回处理结果。
 *
 * @return 处理结果，成功返回true，失败返回false。
 */
bool OperationTurnAroundOnce::UpdatePosProc()
{
	bool ret = false;
	SetPausePreCompile(false);
	Stop();
	DEBUG_OUT("OperationTurnAroundOnce: finish ok,ready to exit current cycle;ready to ForceUpdataCurPos");
	ret = ForceUpdataCurPos();
	return ret;
}

//////////////////////////////////////////////////////

/**
 * @brief 循环处理函数，用于控制AGV的旋转一圈操作。
 *
 * 该函数通过周期性地计算AGV的运动周期时间，以进行速度积分并计算距离。
 * 同时，根据当前阶段执行不同的操作，如开始阶段、旋转阶段、直线运动阶段等。
 * 在执行过程中，会检测是否有异常事件发生，如需要紧急停车等。
 *
 * @return 如果成功完成操作，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::CycleProc()
{
	//
	// 计算运行的周期时间，用来速度积分来计算距离
	//
	TIME_TYPE timeNow = GetTickCount();
	cycle_ = (float)(timeNow - last_time_) / 1000.0f;
	last_time_ = timeNow;

	// 判断是否有异常事件发生，是否需要停车
	uint8_t control_mode = EventOccur(); // CancleCurTask  GetServedEventID();
	bool emg_stop = control_mode == agv_msgs::msg::AGVEvent::ESTOP;
	if (emg_stop) {
		Stop();
		DEBUG_ERROR_OUT("turn around once; event occur,when turn around action");
		return false;
	}

	switch (phase_) {
		// 预先开始阶段，计算相对零点
		case PHASE_READY_START:
		{
			int result = ReadyStartTune();
			switch (result) {
				case RET_NEXT_STAGE:
					SetPhase(PHASE_START);
					break;
					// 其他错误状态
				case RET_GET_DATA_FAILED:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "get camera date time out", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("error:RET_GET_DATA_FAILED_ERROR");
					break;
				default:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "Load status error", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("error:int:result=" << result);
					break;
			}
		}
		break;
		// 开始阶段，读取二维码数据，计算当前循环的运动信息
		case PHASE_START:
		{
			int result = StartTune();
			switch (result) {
				case RET_X_Y_ANGLE_TARGET:
					SetPhase(PHASE_CHECK_X_Y_ANGLE);
					break;
				case RET_X_Y_TARGET:
					SetPhase(PHASE_SECOND_SPIN);
					break;

				case RET_NEXT_STAGE:
					SetPhase(PHASE_FIRST_SPIN);
					break;
				default:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "get camera data time out", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("error:int:result=" << result);
					break;
			}
		}
		break;
		//  进行第一次旋转
		case PHASE_FIRST_SPIN:
			if (FirstSpin()) {
				SetPhase(PHASE_LINE);
				DEBUG_OUT("PHASE_FIRST_SPIN is ok;enter PHASE_LINE");
			}
		
		break;
		//  运动一条直线
		case PHASE_LINE:
			if (MoveLine()) {
				SetPhase(PHASE_SECOND_SPIN);
				DEBUG_OUT("PHASE_LINE is ok;enter PHASE_SECOND_SPIN");
			}

		break;
		// 第二次自旋
		case PHASE_SECOND_SPIN:
			if (SecondSpin()) {
				SetPhase(PHASE_CHECK_X_Y_ANGLE);
				DEBUG_OUT("PHASE_SECOND_SPIN is ok;enter PHASE_CALC_TARGET_ANGLE");
			}
		
		break;
		// 空闲阶段；检测是否需要再次矫正
		case PHASE_CHECK_X_Y_ANGLE:
		{
			int result = CheckXYAngle();
			switch (result) {
				case RET_NEXT_STAGE:
					SetPhase(PHASE_CALC_TARGET_ANGLE);
					break;
				case RET_RESTART:
					SetPhase(PHASE_START);
					break;
				default:
					DEBUG_ERROR_OUT("PHASE_CHECK_X_Y_ANGLE cur result=" << result);
					break;
			}
		}
		break;
		case PHASE_CALC_TARGET_ANGLE:
		{
			int result = CalcTargetAngle();
			switch (result) {
				case RET_ANGLE_TARGET:
					SetPhase(PHASE_UPDATE_POS);
					break;
				case RET_NEXT_STAGE:
					SetPhase(PHASE_THIRD_SPIN);
					DEBUG_OUT("PHASE_CALC_TARGET_ANGLE is ok;enter PHASE_THIRD_SPIN");
					break;
				default:
					break;
			}
		}
		break;
			// 第三次自旋
		case PHASE_THIRD_SPIN:
			if (ThirdSpin()) {
				SetPhase(PHASE_CALC_TARGET_ANGLE);
				DEBUG_OUT("PHASE_THIRD_SPIN is ok;enter PHASE_CALC_TARGET_ANGLE");
			}
		break;

		// 矫正完成更新位姿
		case PHASE_UPDATE_POS:
			if (UpdatePosProc()) {
				SetPhase(PHASE_LOAD);
				DEBUG_OUT("PHASE_UPDATE_POS is ok;enter PHASE_LOAD");
			}
		break;
		// 矫正完成等待退出循环
		case PHASE_LOAD:
			if (HandleLoadAcitonCycle()) {
				SetPhase(PHASE_FINISH);
				DEBUG_OUT("PHASE_LOAD is ok;enter PHASE_FINISH");
			}
		break;
		case PHASE_FINISH:
			return true;
			break;
		default:
			Stop();
			DEBUG_ERROR_OUT("unkown phase");
			break;
	}

	// 日志输出记录
	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	ReadCameraPos(current_camera_type_, x, y, angle);
	return false;
}

/**
 * @brief 检查相机是否丢失数据
 *
 * 检查相机是否丢失数据，如果丢失数据超过预设次数，则启用地面相机数据，并设置任务错误。
 */
void OperationTurnAroundOnce::CheckCameraLostData()
{
	if (!rack_camera_lost_) {
		if (camera_lost_count_++ > CAMERA_LOST_LIMIT) {
			// 如果上读头数据丢失，启用下读头数据
			// 如果不启用下读头，则直接报失败
			rack_camera_lost_ = true;
			DEBUG_ERROR_OUT("Lost camera data, while m_nPhase=" << phase_);
			SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "Lost camera data", agv_msgs::msg::AGVEvent::ALARM);
		}
	}
}

/**
 * @brief 清除相机丢失标志
 *
 * 该函数用于清除相机丢失标志，并重置相关计数器和状态变量。
 *
 * @details
 * - 将 `rack_camera_lost_` 设置为 `false`。
 * - 将 `camera_lost_count_` 重置为 `0`。
 * - 将 `ground_camera_lost` 设置为 `false`。
 * - 打印调试信息，显示当前调整次数和最大调整次数。
 * - 如果当前调整次数小于最大调整次数，则调用 `SetTaskError` 函数设置任务错误状态为无错误。
 */
void OperationTurnAroundOnce::ClearCameraLostFlag()
{
	rack_camera_lost_ = false;
	camera_lost_count_ = 0;
	ground_camera_lost = false;
	DEBUG_OUT("ClearCameraLostFlag;m_nCurAdjustTimes=" << current_adjust_times_ << ",m_nMaxAdjustTimes=" << max_adjust_times_);
	// 在矫正次数允许范围内，允许清除
	if (current_adjust_times_ < max_adjust_times_) {
		SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "no error", agv_msgs::msg::AGVEvent::NONE, false);
	}
}

/**
 * @brief 进行一次旋转移动操作
 *
 * 根据给定的角度进行旋转移动操作。
 *
 * @param angle 目标旋转角度（弧度）
 *
 * @return 如果成功开始旋转或完成旋转，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::SpinMove(float angle)
{
	// 如目标转角已达到
	float diff_angle = angle > 0.0f ? (angle - turned_angle_) : (turned_angle_ - angle);
	if (diff_angle < limit_theta_ / 2) {
		Stop();
		DEBUG_OUT("SpinMove OK;fHaveTurnedAngle=" << turned_angle_ << ",fTargetTotalAngle="
												  << angle << ",fLimitTheta/2=" << limit_theta_ / 2);
		return true;
	}

	float real_angle;
	// 此处使用里程计角度
	if (!GetRelAngle(real_angle)) {
		Stop();
		DEBUG_ERROR_OUT("read odom angle is failed");
		return false;
	}
	real_angle = Angle::NormAngle(real_angle); // [0, 2*PI).

	// SPIN_READY_START 阶段:开始进入旋转，初始化变量
	if (spin_phase_ == SPIN_READY_START) {
		last_real_angle_ = real_angle;
		turned_angle_ = 0.0f;
		micro_angle_diff_ = 0.0f;
		spin_phase_ = SPIN_START_OK;
		DEBUG_OUT("enter SPIN_READY_START once");
		return false;
	}

	// SPIN_START_OK 阶段:开始旋转
	// 累加微小的角度(积分角度)
	micro_angle_diff_ = real_angle - last_real_angle_;
	micro_angle_diff_ = Angle::NormAngle2(micro_angle_diff_); // [-PI, PI).
	if (fabs(micro_angle_diff_) < TO_RADIAN(90)) {			   // 如果4.5*20周期 = 90degree/每秒 则数据跳变
		turned_angle_ += micro_angle_diff_;
		last_real_angle_ = real_angle; // 将当前角度设置为角度记录
		DEBUG_OUT("SpinMove : Enter Turn");
	} else {
		spin_phase_ = SPIN_READY_START;
		DEBUG_ERROR_OUT("SpinMove : fCurRealAngle Jumped;angle is too big");
		Stop();
		return false;
	}

	DEBUG_OUT("SpinMove : fHaveTurnedAngle=" << turned_angle_ << ",fMicroAngleDiff=" << micro_angle_diff_
											 << ",fTargetTotalAngle=" << angle
											 << ",fDiffAngle=" << diff_angle);

	// 如果角度大于10度
	bool use_max_vel = fabs(angle) > TO_RADIAN(10) && fabs(diff_angle) > TO_RADIAN(10);
	float spin_max_vel = fabs(max_spin_vel_);
	float spin_min_vel = fabs(min_spin_vel_);

	float spin_vel = use_max_vel ? spin_max_vel : spin_min_vel;
	spin_vel = __max_(spin_vel, 0.01f);
	float ang_vel = (angle > 0.0f) ? spin_vel : -spin_vel;
	// 输出
	SetAutoModeNavVel(0, 0, ang_vel);
	return false;
}

/**
 * @brief 直线移动函数
 *
 * 根据指定的距离执行直线移动操作。
 *
 * @param distance 直线移动的距离，正值表示前进，负值表示后退。
 *
 * @return 如果完成指定距离的移动，则返回true；否则返回false。
 */
bool OperationTurnAroundOnce::StraightMove(float distance)
{
	bool result = false;
	float vel_x = velocity_;
	vel_x = __max_(vel_x, 0.01f);
	vel_x = distance > 0.0f ? vel_x : -vel_x; // 如果是后退，则速度为负

	// 统计进度
	float vx = 0.0f, vz = 0.0f;
	if (GetRealVel(vx, vz)) {
		current_line_progress_ += vx * cycle_;
	} else {
		current_line_progress_ += vel_x * cycle_;
		DEBUG_ERROR_OUT("get odom speed failed;so force use theory speed");
	}

	DEBUG_OUT("In straight move, current_line_progress = " << current_line_progress_
													<< ", fDist=" << distance << ", cycle = " << cycle_ << ",fVelX=" << vel_x);

	// 如果进度已满，则结束此动作
	if (fabs(current_line_progress_) >= fabs(distance)) {
		vel_x = 0;
		result = true;
		DEBUG_OUT("Line full dist covered: current_line_progress = " << current_line_progress_);
	}

	// 输出速度
	SetAutoModeNavVel(vel_x, 0, 0);
	return result;
}

/**
 * @brief 计算目标角度
 *
 * 从摄像机读取到货架的姿态误差，并根据误差值计算目标角度。
 *
 * @return 返回计算结果的状态码：
 *         - RET_ANGLE_TARGET: 计算成功，角度误差在允许范围内
 *         - RET_NEXT_STAGE: 计算成功，需要继续下一阶段操作
 *         - RET_GET_DATA_FAILED: 获取数据失败
 */
int OperationTurnAroundOnce::CalcTargetAngle()
{
	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	bool read = ReadCameraPos(current_camera_type_, x, y, angle);
	if (read) {
		ClearCameraLostFlag();
		float abs_angle = 0.0f;
		float diff = AngleDiff(abs_angle, angle);
		if (diff <= limit_theta_) {
			Stop();
			DEBUG_OUT("CalcTargetAngle is ok;abs_angle=" << abs_angle
														 << ",angle=" << angle << ",diff=" << diff << ",m_fLimitTheta=" << limit_theta_);
			return RET_ANGLE_TARGET;
		}
		abs_angle = Angle::NormAngle2(abs_angle); //[-PI,PI)
		angle = Angle::NormAngle2(angle);	   //[-PI,PI)
		move_data_.end_spin_angle = Angle::NormAngle2(abs_angle - angle);

		DEBUG_OUT("CalcTargetAngle:abs_angle=" << abs_angle << ",angle=" << angle
											   << ",end_spin_angle=" << move_data_.end_spin_angle);
		return RET_NEXT_STAGE;
	} else {
		Stop();
		CheckCameraLostData();
	}

	return RET_GET_DATA_FAILED;
}

/**
 * @brief 停止操作
 *
 * 该函数用于停止 OperationTurnAroundOnce 操作。
 *
 * @details
 * 调用该函数时，会将自动驾驶模式下的导航速度设置为 (0, 0, 0)，从而实现停车操作。
 *
 */
void OperationTurnAroundOnce::Stop()
{
	DEBUG_OUT("OperationTurnAroundOnce::Stop");
	// 停车
	SetAutoModeNavVel(0, 0, 0);
}

/**
 * @brief 读取当前相机的位置信息
 *
 * 从当前相机类型获取位置信息，并将其存储在提供的变量中。
 *
 * @param pos_x 用于存储 X 坐标的引用变量
 * @param pos_y 用于存储 Y 坐标的引用变量
 * @param angle 用于存储角度的引用变量
 *
 * @return 如果成功读取位置信息，则返回 true；否则返回 false
 */
bool OperationTurnAroundOnce::ReadPos(float &pos_x, float &pos_y, float &angle)
{
	bool ret = ReadCameraPos(current_camera_type_, pos_x, pos_y, angle);
	return ret;
}

/**
 * @brief 从相机读取位置信息
 *
 * 根据传入的类型从相机读取位置信息，包括 x 轴位置、y 轴位置和角度。
 *
 * @param type 相机类型，CAMERA_CHECK_GROUND 表示地面相机，CAMERA_CHECK_RACK 表示货架相机
 * @param pos_x 传出参数，用于存储 x 轴位置
 * @param pos_y 传出参数，用于存储 y 轴位置
 * @param angle 传出参数，用于存储角度
 *
 * @return 成功返回 true，失败返回 false
 */
bool OperationTurnAroundOnce::ReadCameraPos(int type, float &pos_x, float &pos_y, float &angle)
{
	bool ret = false;
	switch (type) {
		case CAMERA_CHECK_GROUND:
			ret = GetGroundCameraData(pos_x, pos_y, angle);
			break;
		case CAMERA_CHECK_RACK:
			ret = GetRackCameraData(pos_x, pos_y, angle);
			break;

		default:
			DEBUG_ERROR_OUT("amr:ReadCameraPos failed:unkown type:" << type);
			break;
	}

	return ret;
}

/**
 * @brief 获取相对角度
 *
 * 从里程计数据中获取相对角度。
 *
 * @param angle 输出参数，存储获取到的相对角度
 * @return 成功获取到数据时返回true，否则返回false
 */
bool OperationTurnAroundOnce::GetRelAngle(float &angle)
{
	float linear_x, angular_z;
	bool ret = GetOdomData(linear_x, angular_z, angle);
	return ret;
}

/**
 * @brief 获取实际速度
 *
 * 从里程计数据中获取实际的线速度 vx 和垂直速度 vz。
 *
 * @param vx 线速度引用，用于存储获取到的线速度值
 * @param vz 垂直速度引用，用于存储获取到的垂直速度值
 *
 * @return 返回布尔值，表示操作是否成功
 *         - true: 操作成功，成功获取到线速度和垂直速度
 *         - false: 操作失败，未能获取到线速度和垂直速度
 */
bool OperationTurnAroundOnce::GetRealVel(float &vx, float &vz)
{
	float angle;
	bool ret = GetOdomData(vx, vz, angle);
	return ret;
}

//////////////////////新修改//////////////////////////
/**
 * @brief 将相对位置转换为相对于给定相机数据的绝对位置
 *
 * 将输入的相对位置转换为相对于给定相机数据的绝对位置，并存储在输出参数中。
 *
 * @param camera_data_pos 包含相机数据的几何消息类型
 * @param input_relative_pos 输入的相对位置
 * @param output_data_pos 存储转换后的绝对位置的输出参数
 *
 * @return 如果转换成功则返回true，否则返回false
 */
bool OperationTurnAroundOnce::ToRelativePos(const geometry_msgs::msg::PoseStamped &camera_data_pos, 
											const Posture &input_relative_pos, Posture &output_data_pos)

{
	float pos_x = input_relative_pos.x_, pos_y = input_relative_pos.y_, angle = input_relative_pos.theta_;

	DEBUG_OUT("amr inputRelativePos:pos_x=" << pos_x << ",pos_y=" << pos_y << ",fPosAngle=" << angle);
	Posture raw_pose;
	if (!RawDataToPos(camera_data_pos, raw_pose)) {
		DEBUG_ERROR_OUT("ToRelativePos RawDataToPos failed");
		return false;
	}

	DEBUG_OUT("ToRelativePos RawDataToPos ok:x=" << raw_pose.x_ << ",y=" << raw_pose.y_ << ",theta=" << raw_pose.theta_);

	Matrix3f T_QrToBase, T_BaseToQr;
	Matrix3f T_QrToRelQr, T_BaseToRelQr, T_RelQrToBase;

	// 原始数据矩阵
	T_QrToBase << cosf(raw_pose.theta_), -sinf(raw_pose.theta_), raw_pose.x_,
		sinf(raw_pose.theta_), cosf(raw_pose.theta_), raw_pose.y_,
		0, 0, 1;

	T_BaseToQr = T_QrToBase.inverse();

	// 需要旋转Qr坐标系变换量:pos_x =pos_y =0;暂时只需要旋转方向
	T_QrToRelQr << cosf(angle), -sinf(angle), pos_x,
		sinf(angle), cosf(angle), pos_y,
		0, 0, 1;

	// Qr相对于车体中心
	T_BaseToRelQr = T_BaseToQr * T_QrToRelQr;

	// 新Qr到Base变换
	T_RelQrToBase = T_BaseToRelQr.inverse();

	Matrix3d RelQrToBase_RM;
	RelQrToBase_RM << T_RelQrToBase(0, 0), T_RelQrToBase(0, 1), 0,
		T_RelQrToBase(1, 0), T_RelQrToBase(1, 1), 0,
		0, 0, 1;

	::Quaterniond QuaterRelQrToBase = ::Quaterniond(RelQrToBase_RM);
	QuaterRelQrToBase.normalize();

	float out_x = T_RelQrToBase(0, 2);
	float out_y = T_RelQrToBase(1, 2);

	geometry_msgs::msg::Pose pose;
	pose.orientation.w = QuaterRelQrToBase.w();
	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = QuaterRelQrToBase.z();
	float out_angle = 0.0f;
	try
	{
		out_angle = tf2::getYaw(pose.orientation);
	} catch (...) {
		DEBUG_ERROR_OUT("amr tf2::getYaw(QtoRelBaseR.orientation) failed");
		return false;
	}
	output_data_pos.x_ = out_x;
	output_data_pos.y_ = out_y;
	output_data_pos.theta_ = out_angle;
	DEBUG_OUT("amr ToRelativePos: fOutX=" << out_x << ",fOutY=" << out_y << ",fOutAngle=" << out_angle);

	return true;
}

/**
 * @brief 将相机数据转换为位置数据
 *
 * 将从相机获取的原始数据转换为位置数据，并存储在输出参数中。
 *
 * @param camera_data_pos 从相机获取的原始位置数据
 * @param output_data_pos 存储转换后的位置数据的结构体
 *
 * @return 转换成功返回true，否则返回false
 */
bool OperationTurnAroundOnce::RawDataToPos(const geometry_msgs::msg::PoseStamped &camera_data_pos, Posture &output_data_pos)
{
	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(camera_data_pos.header.stamp, ROSTime(), CAMERA_DATA_TIME_OUT)) {
		DEBUG_ERROR_OUT("amr Turn around op: GetCameraData time out");
		return false;
	}

	geometry_msgs::msg::Pose data_pos = camera_data_pos.pose;
	output_data_pos.x_ = data_pos.position.x;
	output_data_pos.y_ = data_pos.position.y;
	float tag = data_pos.position.z;
	float data_angle = 0.0f;
	try {
		data_angle = tf2::getYaw(data_pos.orientation);
	} catch (...) {
		DEBUG_ERROR_OUT("amr tf2::getYaw(data_pos.orientation) failed");
		return false;
	}
	output_data_pos.theta_ = data_angle;
	DEBUG_OUT("amr raw data:fTag=" << tag << ",fX=" << output_data_pos.x_ << ",fY=" << output_data_pos.y_ << ",fTheta=" << output_data_pos.theta_);
	return true;
}

/**
 * @brief 将角度 theta 匹配到最接近的 PI/2 的倍数，并返回该倍数。
 *
 * 将输入的角度 theta 规范化到 [0, 2PI] 范围内，然后计算该角度是 PI/2 的几倍（取整数部分），并四舍五入。
 *
 * @param theta 输入的角度，单位为弧度。
 * @return 返回 theta 是 PI/2 的几倍（四舍五入后的结果）。
 */
int OperationTurnAroundOnce::MatchToMultipleOfPi_2(float theta)
{
	//[0,2PI]
	theta = Angle::NormAngle(theta);
	// 计算是pi/2的几倍（取整数部分）四舍五入
	int multiple = static_cast<int>(std::round(theta / PI_2));
	return multiple;
}

/**
 * @brief 计算给定角度与最接近的PI/2倍数之间的余数
 *
 * 给定一个角度theta，该函数计算该角度与最接近的PI/2倍数之间的余数。
 *
 * @param theta 输入的角度，单位：弧度
 * @return 返回theta与最接近的PI/2倍数之间的绝对余数，单位：弧度
 */
float OperationTurnAroundOnce::RemainderDivPi_2(float theta)
{
	//[0,2pi]
	theta = Angle::NormAngle(theta);

	// 几个PI/2 四舍五入
	int n = MatchToMultipleOfPi_2(theta);

	// 取靠近pi/2的最小 余角度(可正负，此处只取绝对值)
	float remainder = theta - n * PI_2;
	return fabs(remainder);
}

/**
 * @brief 获取当前相机原始数据
 *
 * 根据当前相机类型获取对应相机的原始数据
 *
 * @return geometry_msgs::msg::PoseStamped 当前相机的原始数据
 */
geometry_msgs::msg::PoseStamped OperationTurnAroundOnce::GetCurCameraRawData()
{
	const auto &op_data = GetOpData();
	auto ground_camera_pose = op_data.ground_camera_pose_msg;
	auto rack_camera_pose = op_data.rack_camera_pose_msg;

	geometry_msgs::msg::PoseStamped raw_pose;
	switch (current_camera_type_) {
		case CAMERA_CHECK_GROUND:
			raw_pose = ground_camera_pose;
			break;
		case CAMERA_CHECK_RACK:
			raw_pose = rack_camera_pose;
			break;
		default:
			DEBUG_ERROR_OUT("amr GetCurCameraRawData failed:unkown type:" << current_camera_type_);
			break;
	}
	return raw_pose;
}

/**
 * @brief 设置相对位置
 *
 * 该函数用于设置二维码坐标系的相对位置。
 *
 * @return 返回值表示函数执行结果。成功返回RET_NEXT_STAGE，失败则返回相应的错误码。
 */
int OperationTurnAroundOnce::SetRelativePos()
{
	Posture raw_pose;
	// 获取当前使用的相机数据
	auto camera_data = GetCurCameraRawData();
	// 原始数据转换到x y t格式
	if (!RawDataToPos(camera_data, raw_pose)) {
		DEBUG_ERROR_OUT("error:SetRelativePos RawDataToPos time out ");
		return RET_GET_DATA_FAILED;
	}

	float angle = raw_pose.theta_;
	// 匹配pi/2 的整数倍数-四舍五入
	int k = MatchToMultipleOfPi_2(angle);

	// pi/2 余数角度
	float remain_theta = RemainderDivPi_2(angle);

	// 设置二维码坐标系需要旋转的角度方向
	relative_zero_pos_.theta_ = k * PI_2;

	DEBUG_OUT("amr:SetRelativePos; k=" << k << ",remainTheta=" << remain_theta << "relativeZeroPos.fThita=" << relative_zero_pos_.theta_);

	// if (fabs(remain_theta) > TO_RADIAN(max_angle_error_)) {
	// 	DEBUG_ERROR_OUT("error:SetRelativePos falied remainAngle > 30 degree;remainTheta = " << remain_theta);
	// 	return RET_ANGLE_OVER_LIMIT_ERROR;
	// }
	DEBUG_OUT("amr:SetRelativePos ok;relativeAngle=" << relative_zero_pos_.theta_);
	return RET_NEXT_STAGE;
}
